#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <math.h>
using namespace std;
#define START_LINE "***********************\n"
#define DEBUG_ME std::cout

Eigen::Matrix2d Hessian(double x1,double x2)
{
    Eigen::Matrix2d result;
    result << 1200.f*x1*x1-400*x2+2, -400.f*x1,
            -400*x1,               200;
    DEBUG_ME << "H = \n" << result << endl;
    return result;
}

Eigen::Vector2d Jocobian(double x1,double x2)
{

    double temp = x2 - x1*x1;
    Eigen::Vector2d jocobian(-400.f*temp*x1-2+2*x1, 200.f*temp);
    DEBUG_ME << "jocobian:\n" << jocobian.transpose() << endl;
    return jocobian;
}

double f(double x1, double x2)
{
    double temp = (x2 - x1*x1);
    return 100.f*temp*temp + (1.f - x1)*(1.f - x1);
}

double tau(Eigen::Vector2d& pB, Eigen::Vector2d& pU, double trustRegion)
{
    double npB = sqrt(pB.transpose()*pB), npU = sqrt(pU.transpose()*pU);
    if(npB <= trustRegion)
    {
        return 2;
    }
    else if(npU >= trustRegion)
    {
        return trustRegion/npU;
    }
    else
    {
        Eigen::Vector2d pB_U = pB - pU;
        double temp = pU.transpose()*pB_U;
        double temp1 = (pB_U.transpose()*pB_U*(pU.transpose()*pU-trustRegion*trustRegion))(0);
        double tao = (-1*temp + sqrt(temp*temp-temp1)) / (pB_U.transpose()*pB_U);
        return tao+1;
    }
}

Eigen::Vector2d get_pB(Eigen::Matrix2d &hessian, Eigen::Vector2d &jocobian)
{
    return -hessian.inverse()*jocobian;
}

Eigen::Vector2d get_pU(Eigen::Matrix2d &hessian, Eigen::Vector2d &jocobian)
{
    Eigen::Matrix<double,1,2> jocobianT = jocobian.transpose();
    double temp = (-jocobianT*jocobian)(0)/(jocobianT*hessian*jocobian)(0);
    return temp*jocobian;
}

// min $ f = 100(x_2 - x_1^2)^2 + (1 - x_1)^2 $
int solveMinDogleg(void)
{
    double x1_init = 0.1f, x2_init = 0.f;
    double x1 = x1_init, x2 = x2_init;
    double trustRegMax = 10.f, trustReg0 = 0.1f, eta = 0.f;
    double trustRegion = trustReg0;
    int iterator_count = 0, max_interator_limit = 3;
    while(iterator_count<max_interator_limit)
    {
        iterator_count++;
        Eigen::Vector2d jocobian = Jocobian(x1,x2);
        Eigen::Matrix2d hessian  = Hessian(x1,x2) ;
        Eigen::Vector2d pB = get_pB(hessian,jocobian);
        Eigen::Vector2d pU = get_pU(hessian,jocobian);
        DEBUG_ME << "pB = " << pB.transpose() << "\npU = " << pU.transpose() << std::endl;

        double tao = tau(pB,pU,trustRegion);
        DEBUG_ME << "trustRegion = " << trustRegion << ", tau = " << tao << "\n";

        Eigen::Vector2d p;
        if(tao>=0 && tao<1)
        {
            p = tao*pU;
        }
        else if(tao>=1 && tao<=2)
        {
            p = pU + (tao-1)*(pB - pU);
        }
        double step = fabs(jocobian(0))+fabs(jocobian(1));
        DEBUG_ME << "p = " << p.transpose() << std::endl;

        if(step<1e-5) break;
        Eigen::Vector2d currentP(x1+p(0),x2+p(1));
        double a = f(x1,x2) - f(currentP(0),currentP(1));
        double b = (-1.f*jocobian.transpose()*p-0.5*p.transpose()*hessian*p)(0);
        double rho = a/b;
        DEBUG_ME << "a = " << a << " b = " << b << std::endl;
        if(rho<0.25)
        {
            trustRegion *= 0.25;
        }
        else if(rho>0.75 && fabs(p.norm() - trustRegion)<1e-7)
        {
            trustRegion = std::min(2*trustRegion,trustRegMax);
        }
        else
        {
            DEBUG_ME << "normal mode, not change on trustRegion!\n";
        }
        //        DEBUG_ME << "rho = " << rho << " eta = " << eta << endl;
        if(rho>eta)
        {
            x1 += p(0), x2 += p(1);
        }
        DEBUG_ME << iterator_count << ": " << x1 << " " << x2 << std::endl << START_LINE << std::endl;
    }
    std::cout << iterator_count << ": " << x1 << " " << x2 << std::endl;
}

int main(int argc, char *argv[])
{
    solveMinDogleg();
    return 0;
}
